/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.templates.RobotState;
import edu.wpi.first.wpilibj.templates.subsystems.NewPIDLifterSubsystem;

/**
 *
 * @author matt
 */
public class newDriveDirection extends CommandBase {

    RobotState robotState = RobotState.getInstance();

    public static class Direction {

        public final int value;
        // direction enumeration constants
        protected static final int m_kLeft = 0;
        protected static final int m_kRight = 1;
        protected static final int m_kFore = 2;
        protected static final int m_kRear = 3;
        public static final Direction kLeft = new Direction(m_kLeft);
        public static final Direction kRight = new Direction(m_kRight);
        public static final Direction kFore = new Direction(m_kFore);
        public static final Direction kRear = new Direction(m_kRear);

        private Direction(int direction) {
            this.value = direction;
        }
    }
    boolean m_isRunning;
    private Direction commandDirection;

    public newDriveDirection(final Direction direction) {
        // Use requires() here to declare subsystem dependencies
        // eg. requires(chassis);
        requires(leftDriveSubsystem);
        requires(rightDriveSubsystem);
        requires(swerveSubsystem);
        requires(leftForeLifterSubsystem);
        requires(leftRearLifterSubsystem);
        requires(rightForeLifterSubsystem);
        requires(rightRearLifterSubsystem);

        commandDirection = direction;
        m_isRunning = false;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
        m_isRunning = true;

        // reinit 
        leftDriveSubsystem.setSpeedMode();
        rightDriveSubsystem.setSpeedMode();
        armSubsystem.initialize();
        swerveSubsystem.recover(true);
        
        // command drives to slow to the point where we can change direction
        leftDriveSubsystem.drive(0);
        rightDriveSubsystem.drive(0);
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        if (/*
                 * left and right are slow enough
                 */true) {

            switch (commandDirection.value) {
                case Direction.m_kFore:
                    System.out.println("Fore");
                    // Set the new drive directions
                    leftDriveSubsystem.setReverse(false);
                    rightDriveSubsystem.setReverse(false);
                    // Retract the Swerve Mechanism
                    swerveSubsystem.raiseSwerve();
                    robotState.setDirection(RobotState.RobotDirection.kForward);
                    rightRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalLowered);
                    rightForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalRaised);
                    leftRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalLowered);
                    leftForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalRaised);
                    break;
                case Direction.m_kRear:
                    System.out.println("Rear");
                    // Set the new drive directions
                    leftDriveSubsystem.setReverse(true);
                    rightDriveSubsystem.setReverse(true);
                    // Retract the Swerve Mechanism
                    swerveSubsystem.raiseSwerve();
                    robotState.setDirection(RobotState.RobotDirection.kReverse);
                    rightRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalRaised);
                    rightForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalLowered);
                    leftRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalRaised);
                    leftForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveNormalLowered);
                    break;
                case Direction.m_kLeft:
                    System.out.println("Left");
                    // Set the new drive directions
                    leftDriveSubsystem.setReverse(true);
                    rightDriveSubsystem.setReverse(true);
                    // Deploy the Swerve Mechanism
                    swerveSubsystem.lowerSwerve();
                    robotState.setDirection(RobotState.RobotDirection.kLeft);
//                    rightRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveLowered);
//                    rightForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveLowered);
                    rightRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    rightForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    leftRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    leftForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    break;
                case Direction.m_kRight:
                    System.out.println("Right");
                    // Set the new drive directions
                    leftDriveSubsystem.setReverse(false);
                    rightDriveSubsystem.setReverse(false);
                    // Deploy the Swerve Mechanism
                    swerveSubsystem.lowerSwerve();
                    robotState.setDirection(RobotState.RobotDirection.kRight);
                    rightRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    rightForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    leftRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
                    leftForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveRaised);
//                    leftRearLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveLowered);
//                    leftForeLifterSubsystem.setLifterPosition(NewPIDLifterSubsystem.LifterPresetLocation.kDriveSwerveLowered);
                    break;
            }
            //m_isRunning = false;            
        }
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        // should really be when we are fully changed over
        return m_isRunning;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
